#include "msg_convert.h"
#include <pcl_conversions/pcl_conversions.h> // For pcl::toROSMsg
#include <pcl/common/transforms.h> // For pcl::transformPointCloud
#include <pcl/common/common.h>  // For pcl::concatenatePointCloud

namespace msg_convert{

    // pcl::PointCloud<pcl::PointXYZ>::Ptr -> sensor_msgs::PointCloud2
    sensor_msgs::PointCloud2::Ptr toPointCloud2(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, string frame_id){
        if(!cloud) return nullptr;
        sensor_msgs::PointCloud2::Ptr pointcloud2_ptr = boost::make_shared<sensor_msgs::PointCloud2>();
        pcl::toROSMsg(*cloud, *pointcloud2_ptr);
        pointcloud2_ptr->header.frame_id = frame_id;
        pcl_conversions::fromPCL(cloud->header.stamp, pointcloud2_ptr->header.stamp);
        // pointcloud2_ptr->header.stamp = ros::Time(cloud->header.stamp); 
        return pointcloud2_ptr;
    }

    // sensor_msgs::PointCloud2::Ptr -> pcl::PointCloud<pcl::PointXYZ>::Ptr
    pcl::PointCloud<pcl::PointXYZ>::Ptr toPointCloudXYZ(sensor_msgs::PointCloud2::Ptr pointcloud2_ptr){
        if(!pointcloud2_ptr) return nullptr;
        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::fromROSMsg(*pointcloud2_ptr, *pcl_cloud_ptr);
        return pcl_cloud_ptr;
    }

    // sensor_msgs::LaserScan::Ptr -> pcl::PointCloud<pcl::PointXYZ>::Ptr
    pcl::PointCloud<pcl::PointXYZ>::Ptr toPointCloudXYZ(sensor_msgs::LaserScan::Ptr scan_ptr){
        if(!scan_ptr) return nullptr;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>());
        for(size_t i=0; i<scan_ptr->ranges.size(); ++i){
            float range = scan_ptr->ranges[i];
            // 跳过无效的距离值
            if(std::isinf(range) || std::isnan(range)){
                continue;
            }
            float angle = scan_ptr->angle_min + i * scan_ptr->angle_increment;
            pcl::PointXYZ point;
            point.x = range * cos(angle);
            point.y = range * sin(angle);
            point.z = 0.0f;
            cloud_ptr->points.emplace_back(point);
        }
        cloud_ptr->header.frame_id = scan_ptr->header.frame_id; // 设置坐标系
        // cloud_ptr->header.stamp = scan_ptr->header.stamp.toNSec();
        pcl_conversions::toPCL(scan_ptr->header.stamp, cloud_ptr->header.stamp);
        cloud_ptr->height = 1; // 单层点云
        cloud_ptr->width = cloud_ptr->points.size();  
        cloud_ptr->is_dense = false;    
        return cloud_ptr; 
    }

    // pcl::PointCloud<pcl::PointXYZ>::Ptr -> sensor_msgs::LaserScan::Ptr
    sensor_msgs::LaserScan::Ptr toLaserScan(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr){
        if(!cloud_ptr) return nullptr;
        sensor_msgs::LaserScan::Ptr scan_ptr(new sensor_msgs::LaserScan());
        scan_ptr->header.frame_id = cloud_ptr->header.frame_id; // 设置坐标系
        int laser_size = 1080;
        pcl_conversions::fromPCL(cloud_ptr->header.stamp, scan_ptr->header.stamp);
        scan_ptr->angle_min = -M_PI;
        scan_ptr->angle_max = M_PI;
        scan_ptr->angle_increment = (scan_ptr->angle_max - scan_ptr->angle_min) / (laser_size - 1);
        scan_ptr->time_increment = 0.0;
        scan_ptr->scan_time = 0.0;
        scan_ptr->range_min = 0.0;
        scan_ptr->range_max = 100.0;
        scan_ptr->ranges.assign(laser_size, std::numeric_limits<float>::infinity());
        scan_ptr->intensities.assign(laser_size, std::numeric_limits<float>::infinity());
        for(size_t i=0; i<cloud_ptr->points.size(); ++i){
            float range = hypot(cloud_ptr->points[i].x, cloud_ptr->points[i].y);
            float angle = atan2(cloud_ptr->points[i].y, cloud_ptr->points[i].x);
            int index = (int)((angle - scan_ptr->angle_min) / scan_ptr->angle_increment);
            if(index < 0 || index > laser_size){
                continue;
            }
            scan_ptr->ranges[index] = min(scan_ptr->ranges[index], range);
        }
        return scan_ptr;
    }
};